
#include "graphics/frustum.h"
#include "graphics/renderer.h"

namespace dw
{
namespace graphics
{

																	Frustum::Frustum(const float& fov, const float& aspectRatio, const float& nearClipDist, const float& farClipDist)
{
	//float h = _nearClipDist * tanf(fov / 2.0f);
	//float w = h * aspectRatio;

	//cout << h << endl;
	//cout << w << endl;

	/*_planes[0] = Plane(Vector3f(0, 1, -h),	Point3f(0, 0, 0));
	_planes[1] = Plane(Vector3f(0, -1, -h), Point3f(0, 0, 0));
	_planes[2] = Plane(Vector3f(1, 0, -w),	Point3f(0, 0, 0));
	_planes[3] = Plane(Vector3f(-1, 0, -w), Point3f(0, 0, 0));
	_planes[4] = Plane(Vector3f(0, 0, -1),	Point3f(0, 0, -_nearClipDist));
	_planes[5] = Plane(Vector3f(0, 0, 1),		Point3f(0, 0, _farClipDist));*/

	//Matrix44f proj, view, m;
	//glGetFloatv(GL_PROJECTION_MATRIX, &proj[0]);
	//glGetFloatv(GL_MODELVIEW_MATRIX, &view[0]);
	//std::cout << m << endl;

	//m = proj;

	//_planes[0] = Plane(m(3, 0) + m(0, 0), m(3, 1) + m(0, 1), m(3, 2) + m(0, 2), m(3, 3) + m(0, 3));
	//_planes[1] = Plane(m(3, 0) - m(0, 0), m(3, 1) - m(0, 1), m(3, 2) - m(0, 2), m(3, 3) - m(0, 3));
	//_planes[2] = Plane(m(3, 0) - m(1, 0), m(3, 1) - m(1, 1), m(3, 2) - m(1, 2), m(3, 3) - m(1, 3));
	//_planes[3] = Plane(m(3, 0) + m(1, 0), m(3, 1) + m(1, 1), m(3, 2) + m(1, 2), m(3, 3) + m(1, 3));
	//_planes[4] = Plane(m(3, 0) + m(2, 0), m(3, 1) + m(2, 1), m(3, 2) + m(2, 2), m(3, 3) + m(2, 3));
	//_planes[5] = Plane(m(3, 0) - m(2, 0), m(3, 1) - m(2, 1), m(3, 2) - m(2, 2), m(3, 3) - m(2, 3));

	float ProjM[16];
	float ModM[16];
	float Clip[16];

	glGetFloatv(GL_PROJECTION_MATRIX,	ProjM);
	glGetFloatv(GL_MODELVIEW_MATRIX,	ModM);

	Clip[ 0] = ModM[ 0] * ProjM[ 0] + ModM[ 1] * ProjM[ 4] + ModM[ 2] * ProjM[ 8] + ModM[ 3] * ProjM[12];
	Clip[ 1] = ModM[ 0] * ProjM[ 1] + ModM[ 1] * ProjM[ 5] + ModM[ 2] * ProjM[ 9] + ModM[ 3] * ProjM[13];
	Clip[ 2] = ModM[ 0] * ProjM[ 2] + ModM[ 1] * ProjM[ 6] + ModM[ 2] * ProjM[10] + ModM[ 3] * ProjM[14];
	Clip[ 3] = ModM[ 0] * ProjM[ 3] + ModM[ 1] * ProjM[ 7] + ModM[ 2] * ProjM[11] + ModM[ 3] * ProjM[15];
	Clip[ 4] = ModM[ 4] * ProjM[ 0] + ModM[ 5] * ProjM[ 4] + ModM[ 6] * ProjM[ 8] + ModM[ 7] * ProjM[12];
	Clip[ 5] = ModM[ 4] * ProjM[ 1] + ModM[ 5] * ProjM[ 5] + ModM[ 6] * ProjM[ 9] + ModM[ 7] * ProjM[13];
	Clip[ 6] = ModM[ 4] * ProjM[ 2] + ModM[ 5] * ProjM[ 6] + ModM[ 6] * ProjM[10] + ModM[ 7] * ProjM[14];
	Clip[ 7] = ModM[ 4] * ProjM[ 3] + ModM[ 5] * ProjM[ 7] + ModM[ 6] * ProjM[11] + ModM[ 7] * ProjM[15];
	Clip[ 8] = ModM[ 8] * ProjM[ 0] + ModM[ 9] * ProjM[ 4] + ModM[10] * ProjM[ 8] + ModM[11] * ProjM[12];
	Clip[ 9] = ModM[ 8] * ProjM[ 1] + ModM[ 9] * ProjM[ 5] + ModM[10] * ProjM[ 9] + ModM[11] * ProjM[13];
	Clip[10] = ModM[ 8] * ProjM[ 2] + ModM[ 9] * ProjM[ 6] + ModM[10] * ProjM[10] + ModM[11] * ProjM[14];
	Clip[11] = ModM[ 8] * ProjM[ 3] + ModM[ 9] * ProjM[ 7] + ModM[10] * ProjM[11] + ModM[11] * ProjM[15];
	Clip[12] = ModM[12] * ProjM[ 0] + ModM[13] * ProjM[ 4] + ModM[14] * ProjM[ 8] + ModM[15] * ProjM[12];
	Clip[13] = ModM[12] * ProjM[ 1] + ModM[13] * ProjM[ 5] + ModM[14] * ProjM[ 9] + ModM[15] * ProjM[13];
	Clip[14] = ModM[12] * ProjM[ 2] + ModM[13] * ProjM[ 6] + ModM[14] * ProjM[10] + ModM[15] * ProjM[14];
	Clip[15] = ModM[12] * ProjM[ 3] + ModM[13] * ProjM[ 7] + ModM[14] * ProjM[11] + ModM[15] * ProjM[15];

	// right
	_planes[0] = Plane(Clip[ 3] - Clip[ 0], Clip[ 7] - Clip[ 4], Clip[11] - Clip[ 8], Clip[15] - Clip[12]);
	// left
	_planes[1] = Plane(Clip[ 3] + Clip[ 0], Clip[ 7] + Clip[ 4], Clip[11] + Clip[ 8], Clip[15] + Clip[12]);
	// bottom
	_planes[2] = Plane(Clip[ 3] + Clip[ 1], Clip[ 7] + Clip[ 5], Clip[11] + Clip[ 9], Clip[15] + Clip[13]);
	// top
	_planes[3] = Plane(Clip[ 3] - Clip[ 1], Clip[ 7] - Clip[ 5], Clip[11] - Clip[ 9], Clip[15] - Clip[13]);
	// back
	_planes[4] = Plane(Clip[ 3] - Clip[ 2], Clip[ 7] - Clip[ 6], Clip[11] - Clip[10], Clip[15] - Clip[14]);
	// front
	_planes[5] = Plane(Clip[ 3] + Clip[ 2], Clip[ 7] + Clip[ 6], Clip[11] + Clip[10], Clip[15] + Clip[14]);


	//// focal length
	//float l = 1 / std::tan(fov / 2.0f);
	//float a = aspectRatio;

	//_planes[PLANE_NEAR]		= Plane(Vector3f(0, 0, -1),	Point3f(0, 0, nearClipDist));
	//_planes[PLANE_FAR]		= Plane(Vector3f(0, 0, 1), Point3f(0, 0, farClipDist));
	//_planes[PLANE_LEFT]		= Plane(Vector3f(l / std::sqrt(l * l + 1), 0, -1 / std::sqrt(l * l + 1)),	Point3f(0, 0, 0));
	//_planes[PLANE_RIGHT]	= Plane(Vector3f(-l / std::sqrt(l * l + 1), 0, -1 / std::sqrt(l * l + 1)), Point3f(0, 0, 0));
	//_planes[PLANE_TOP]		= Plane(Vector3f(0, -l / std::sqrt(l * l + a * a), -a / std::sqrt(l * l + a * a)), Point3f(0, 0, 0));
	//_planes[PLANE_BOTTOM] = Plane(Vector3f(0, l / std::sqrt(l * l + a * a), -a / std::sqrt(l * l + a * a)),	Point3f(0, 0, 0));
}

																	Frustum::Frustum(const Matrix44f& m)
{
	/*_planes[0] = Plane(-m.m03 - m.m00, -m.m13 - m.m10, -m.m23 - m.m20, m.m33 + m.m30);
	_planes[1] = Plane(m.m00 - m.m03, m.m10 - m.m13, m.m20 - m.m23,	-m.m30 + m.m33);
	_planes[2] = Plane(-m.m03 - m.m01, -m.m13 - m.m11, -m.m23 - m.m21, m.m33 + m.m31);
	_planes[3] = Plane(m.m01 - m.m03, m.m11 - m.m13, m.m21 - m.m23, -m.m31 + m.m33);
	_planes[4] = Plane(m.m02 - m.m03, m.m12 - m.m13, m.m22 - m.m23, -m.m32 + m.m33);
	_planes[5] = Plane(-m.m02, -m.m12, -m.m22, m.m32);*/
}

const Plane& Frustum::getPlane(const unsigned i) const
{
	if (i >= 6) throw std::out_of_range(__FUNCTION__);

	return _planes[i];
}

void Frustum::getVertices(std::vector< Point3f >& vertices) const
{
	Point3f intersection;

	if (Plane::intersection(_planes[2], _planes[4], _planes[0], intersection))
	{
		vertices.push_back(intersection);
	}
	if (Plane::intersection(_planes[3], _planes[4], _planes[0], intersection))
	{
		vertices.push_back(intersection);
	}
	if (Plane::intersection(_planes[3], _planes[5], _planes[0], intersection))
	{
		vertices.push_back(intersection);
	}
	if (Plane::intersection(_planes[2], _planes[5], _planes[0], intersection))
	{
		vertices.push_back(intersection);
	}
	if (Plane::intersection(_planes[2], _planes[4], _planes[1], intersection))
	{
		vertices.push_back(intersection);
	}
	if (Plane::intersection(_planes[3], _planes[4], _planes[1], intersection))
	{
		vertices.push_back(intersection);
	}
	if (Plane::intersection(_planes[3], _planes[5], _planes[1], intersection))
	{
		vertices.push_back(intersection);
	}
	if (Plane::intersection(_planes[2], _planes[5], _planes[1], intersection))
	{
		vertices.push_back(intersection);
	}
}

bool Frustum::intersects(const Point3f& point) const
{
	for (unsigned i = 0; i < 6; ++i)
	{
		if (Plane::distance(_planes[i], point) >= 0.f)
			return false;
	}
	return true;
}

bool Frustum::intersects(const BSphere& bsphere) const
{
	// DW_NOT_IMPLEMENTED;
	return false;
}

bool Frustum::intersects(const AABBox& aabb, IntersectionInfo& intersectionInfo) const
{
	std::vector< Point3f >						points;
	unsigned count = 0;

	aabb.getVertices(points);
	for (std::vector< Point3f >::iterator it = points.begin(); it != points.end(); ++it)
	{
		if (this->intersects(*it))
			count++;
	}
	if (count == 0)
	{
		intersectionInfo.type = IntersectionInfo::OUTSIDE;
		return false;
	}
	if (count < 6)
	{
		intersectionInfo.type = IntersectionInfo::INTERSECT;
		return true;
	}
	intersectionInfo.type = IntersectionInfo::INSIDE;
	return true;
}

bool Frustum::intersects(const AABBox& aabb) const
{
	IntersectionInfo intersectionInfo;

	return this->intersects(aabb, intersectionInfo);
}

void Frustum::transform(const Matrix44f& m)
{
	for (unsigned i = 0; i < 6; ++i)
		_planes[i] = Plane::transform(_planes[i], m);
}

void Frustum::draw()
{
	std::vector< Point3f > vertices;

	this->getVertices(vertices);

	glColor3f(1.f, 0.f, 0.f);

	glBegin(GL_QUADS);

	glVertex3f(vertices[0].x, vertices[0].y, vertices[0].z);
	glVertex3f(vertices[1].x, vertices[1].y, vertices[1].z);
	glVertex3f(vertices[2].x, vertices[2].y, vertices[2].z);
	glVertex3f(vertices[3].x, vertices[3].y, vertices[3].z);

	glVertex3f(vertices[4].x, vertices[4].y, vertices[4].z);
	glVertex3f(vertices[5].x, vertices[5].y, vertices[5].z);
	glVertex3f(vertices[6].x, vertices[6].y, vertices[6].z);
	glVertex3f(vertices[7].x, vertices[7].y, vertices[7].z);

	glEnd();
	glBegin(GL_LINES);
	glVertex3f(vertices[0].x, vertices[0].y, vertices[0].z); glVertex3f(vertices[4].x, vertices[4].y, vertices[4].z);
	glVertex3f(vertices[1].x, vertices[1].y, vertices[1].z); glVertex3f(vertices[5].x, vertices[5].y, vertices[5].z);
	glVertex3f(vertices[2].x, vertices[2].y, vertices[2].z); glVertex3f(vertices[6].x, vertices[6].y, vertices[6].z);
	glVertex3f(vertices[3].x, vertices[3].y, vertices[3].z); glVertex3f(vertices[7].x, vertices[7].y, vertices[7].z);
	glEnd();

	glColor3f(1.f, 1.f, 1.f);
}

} // namespace graphics
} // namespace dw
